#!/usr/bin/env python
#coding:utf-8
import os
import rospy
import keyboard
import threading
from math import pi
from windows_communicator.msg import MzNavState

LOOP_RATE = 10  # [HZ]
StateDict = {0:"STOP", 1:"MOVE_WITH_FACE_DETECT", 2:"MOVE_WITHOUT_FACE_DETECT"}
TARGET_POSES = [(0.000, 0.000, pi/180*0), 
                (-2.59, -4.83, pi/180*45),  # 付款处
                (-7.22, 18.67, pi/180*150), # 妇儿路口
                (5.34, 34.33, pi/180*150),  # 妇科护士站
                (-24.7, -0.28, pi/180*30)]  # 儿科护士站
# TARGET_POSES = [(0.000, 0.000, pi/180*0), 
#                 (0.386, 7.669, pi/180*90), 
#                 (0.324, -20.3, pi/180*-90), 
#                 (11.69, -28.7, pi/180*-90), 
#                 (28.91, -24.2, pi/180*170)]  # [m][rad]

class FakePubber():
    working_state = 0
    target_pos = TARGET_POSES[0]
    face_ratio = 0
    if_pub = False
    pubber = rospy.Publisher('nav_state', MzNavState, queue_size=10)

    def __init__(self):
        self.PrintState()
        pub_thread = threading.Thread(target=self.Publish)
        pub_thread.start()
        keyboard.hook(self.KeyboardHandler)
        keyboard.wait()

    def KeyboardHandler(self, evt):
        if evt.event_type == 'up' and evt.name == 'esc':
            os._exit(0)
        elif evt.event_type == 'up' and evt.name == 'space':
            self.if_pub = 1 - self.if_pub
        elif evt.event_type == 'down' and (evt.name == 'i' or evt.name == 'I'):
            self.face_ratio += 0.05
        elif evt.event_type == 'down' and (evt.name == 'k' or evt.name == 'K'):
            self.face_ratio -= 0.05
        elif evt.event_type == 'up' and evt.name == '1':
            self.target_pos = TARGET_POSES[0]
        elif evt.event_type == 'up' and evt.name == '2':
            self.target_pos = TARGET_POSES[1]
        elif evt.event_type == 'up' and evt.name == '3':
            self.target_pos = TARGET_POSES[2]
        elif evt.event_type == 'up' and evt.name == '4':
            self.target_pos = TARGET_POSES[3]
        elif evt.event_type == 'up' and evt.name == '5':
            self.target_pos = TARGET_POSES[4]
        elif evt.event_type == 'up' and (evt.name == 'q' or evt.name == 'Q'):
            self.working_state = 0
        elif evt.event_type == 'up' and (evt.name == 'w' or evt.name == 'W'):
            self.working_state = 1
        elif evt.event_type == 'up' and (evt.name == 'e' or evt.name == 'E'):
            self.working_state = 2
        self.PrintState()

    def PrintState(self):
        os.system("clear")  # win则为cls
        specification = "12345选择目标点、QWE切换状态、IK调节face_ratio、空格暂停/开始发布、Esc退出"
        print(specification)
        if_pub = "True" if self.if_pub else "False"
        print("if pub: [ " + if_pub + " ]")
        working_state = "working_state: " + StateDict[self.working_state]
        print(working_state)
        target_pos = "target_pos: [x:{:.2f}, y:{:.2f}, theta:{:.2f}]".format(self.target_pos[0], self.target_pos[1], self.target_pos[2])
        print(target_pos)
        face_ratio = "face_ratio: {:.3f}".format(self.face_ratio)
        print(face_ratio)

    def Publish(self):
        '''循环发布msg'''
        rate = rospy.Rate(LOOP_RATE)
        while not rospy.is_shutdown():
            if(self.if_pub):
                pub_data = MzNavState(self.working_state, self.target_pos[0], self.target_pos[1], self.target_pos[2], self.face_ratio)
                self.pubber.publish(pub_data)
            rate.sleep()

if __name__ == "__main__":
    rospy.init_node('fake_win_communicator', anonymous=True)
    FakePubber()
